import Interface.BluetoothNXT;
import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;

public class WallFollowerV3 {
	
	private float leftWheelDiameter; 
	private float rightWheelDiameter;
	private float trackWidth = 164;
	private Motor leftMotor = Motor.B;
	private Motor rightMotor = Motor.C;
	private TachoPilot pilot;
	private UltrasonicSensor sonicSensor;
	private boolean stop;
	
	private int distanceFromWall = 26;
	private int offset = 3;
	
	public WallFollowerV3(){
		this.setLeftWheelDiameter(56);
		this.setRightWheelDiameter(56);
		this.setTrackWidth(164);
		this.setLeftMotor(Motor.B);
		this.setRightMotor(Motor.C);
		setStop(false);
		pilot = new TachoPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		sonicSensor = new UltrasonicSensor(SensorPort.S2);
	}
	
	public WallFollowerV3(float leftWheelDiameter, float rightWheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor){
		this.setLeftWheelDiameter(leftWheelDiameter);
		this.setRightWheelDiameter(rightWheelDiameter);
		this.setTrackWidth(trackWidth);
		this.setLeftMotor(leftMotor);
		this.setRightMotor(rightMotor);
		setStop(false);
		pilot = new TachoPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
	}
	
	public void action(){			
		connectToBluetooth();
		
		while(!shouldStop()){
			LCD.drawString("Dist:" + sonicSensor.getDistance(), 0, 0);
			if(sonicSensor.getDistance() > 70){
				LCD.drawString("Muur weg", 0, 1);
				getPilot().travel(45);
				getPilot().rotate(-10);
			}else if(binnenInterval(sonicSensor, distanceFromWall, offset)){
				LCD.drawString("Juist", 0, 1);
				getPilot().forward();
			}else if(teVer(sonicSensor, distanceFromWall, offset)){
				LCD.drawString("teVer", 0, 1);
				getPilot().stop();
				getPilot().rotate(-7);
				getPilot().travel(25);
			}else if(teDicht(sonicSensor, distanceFromWall, offset)){
				LCD.drawString("teDicht", 0, 1);
				getPilot().stop();
				getPilot().rotate(7);
				getPilot().travel(10);
			}
			LCD.clearDisplay();
		}
	}

	public void connectToBluetooth() {
		LCD.drawString("Connect Bluetooth?", 0, 1);
		int button = Button.waitForPress();
		if (button == Button.ID_ENTER){		
				BluetoothNXT test = new BluetoothNXT(getPilot(),new LightSensor(SensorPort.S1), sonicSensor);
				test.start();
		}
	}
	
	private static boolean binnenInterval(UltrasonicSensor sonicSensor, int distanceFromWall, int offset){
		boolean kleinerDanMax = sonicSensor.getDistance() < distanceFromWall + offset;
		boolean groterDanMin = sonicSensor.getDistance() > distanceFromWall - offset;
		return kleinerDanMax && groterDanMin;
	}
	
	private static boolean teVer(UltrasonicSensor sonicSensor, int distanceFromWall, int offset){
		boolean teVer =sonicSensor.getDistance() >= distanceFromWall + offset;
		return teVer;
	}
	
	private static boolean teDicht(UltrasonicSensor sonicSensor, int distanceFromWall, int offset){
		boolean teDicht = sonicSensor.getDistance() <= distanceFromWall - offset;		
		return teDicht;
	}
	

	private void setTrackWidth(float trackWidth) {
		this.trackWidth = trackWidth;	
		
	}
	private void setLeftMotor(Motor leftMotor) {
		this.leftMotor = leftMotor;	
		
	}
	private void setRightMotor(Motor rightMotor) {
		this.rightMotor = rightMotor;	
		
	}
	private void setRightWheelDiameter(float rightWheelDiameter) {
		this.rightWheelDiameter = rightWheelDiameter;	
		
	}
	private void setLeftWheelDiameter(float leftWheelDiameter) {
		this.leftWheelDiameter = leftWheelDiameter;		
	}
	
	public TachoPilot getPilot(){
		return this.pilot;
	}
	
	public UltrasonicSensor getSensor(){
		return this.sonicSensor;
	}

	public boolean shouldStop() {
		return stop;
	}

	public void setStop(boolean stop) {
		this.stop = stop;
	}
}





